import serial

class Link:
    FRAME_HEADER    = 0xA5
    FRAME_TAIL      = 0x5A

    MSG_OK              = 0

    MSG_POS_GLOBAL      = 1
    MSG_POS_RELATIVE    = 3

    MSG_VIEW_GLOBAL     = 2
    MSG_VIEW_RELATIVE   = 4

    def __init__(self, port = '/dev/ttyS5', baud = 115200, timeout = 0.1):
        self._ser = serial.Serial(port, baud, timeout = timeout)

    def send(self, msg):
        self._ser.write(msg)

    def read(self, n = 1):
        bytes = self._ser.read(n)
        return bytes


    @staticmethod
    def pack_pos_global(x, y, extra = 120):
        x = int(x*10).to_bytes(2, 'little', signed=True)
        y = int(y*10).to_bytes(2, 'little', signed=True)
        z = int(extra).to_bytes(1, 'little', signed=False)
        
        msg = [
            Link.FRAME_HEADER, 
            Link.MSG_POS_GLOBAL, 
            x[0], x[1], y[0], y[1], 
            z[0], Link.FRAME_TAIL
        ]

        return msg

    @staticmethod
    def pack_pos_relative(dx, dy):
        dx = int(dx*10).to_bytes(2, 'little', signed=True)
        dy = int(dy*10).to_bytes(2, 'little', signed=True)

        msg = [
            Link.FRAME_HEADER, 
            Link.MSG_POS_RELATIVE, 
            dx[0], dx[1], dy[0], dy[1], 
            0xF0, Link.FRAME_TAIL
        ]
        return msg

    @staticmethod
    def pack_view_global(z, yaw, extra = 0xF0):
        '''yaw注意正负，单位角度'''
        z = int(z*10).to_bytes(2, 'little', signed=True)
        yaw = int(yaw*10).to_bytes(2, 'little', signed=True)

        msg = [
            Link.FRAME_HEADER, 
            Link.MSG_VIEW_GLOBAL, 
            z[0], z[1], yaw[0], yaw[1], 
            0xF0, Link.FRAME_TAIL
        ]
        return msg

    @staticmethod
    def pack_view_relative(dz, dyaw, extra = 0xF0):
        '''dz: cm, dyaw: deg'''
        dz = int(dz*10).to_bytes(2, 'little', signed=True)
        dyaw = int(dyaw*10).to_bytes(2, 'little', signed=True)

        msg = [
            Link.FRAME_HEADER, 
            Link.MSG_VIEW_RELATIVE, 
            dz[0], dz[1], dyaw[0], dyaw[1], 
            0xF0, Link.FRAME_TAIL
        ]
        return msg
    
    @staticmethod
    def pack_ok(isOk = 0x0F):
        msg = [
            Link.FRAME_HEADER, Link.MSG_OK,
            0x00, 0x00, 0x00, 0x00, 
            isOk, 
            Link.FRAME_TAIL
        ]
        return msg